import launch
import launch.event_handlers
import launch.launch_description_sources
import launch_ros
import os
from ament_index_python import get_package_share_directory
import launch_ros.parameter_descriptions

def generate_launch_description():
    #获取文件路径
    package_path=get_package_share_directory('robot_sim')
    default_urdf_path=os.path.join(package_path,'urdf/robot1','robot1.urdf.xacro')
    default_world_path=os.path.join(package_path,'world','custom_room1.world')
    #为了方便修改将路径设置为参数
    action_declare_arg_model_path=launch.actions.DeclareLaunchArgument(
        name="model",default_value=str(default_urdf_path),description='机器人模型描述文件的路径'
    )

    #通过路径获取文件的内容并且提供给robot_state
    robot_description_param=launch.substitutions.Command(
    ['xacro ', launch.substitutions.LaunchConfiguration('model')]
    )

    parameter_value=launch_ros.parameter_descriptions.ParameterValue(
        robot_description_param,value_type=str)
    print(parameter_value)

    robot_state_publisher=launch_ros.actions.Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        parameters=[{'robot_description':parameter_value}],
    )

    action_gazebo_launch=launch.actions.IncludeLaunchDescription(
        launch.launch_description_sources.PythonLaunchDescriptionSource(
            [get_package_share_directory('gazebo_ros'),'/launch','/gazebo.launch.py']
        ),
        launch_arguments=[('world',default_world_path),('verbose','true')]
    )
    launch_spawn_entity=launch_ros.actions.Node(
        package='gazebo_ros',
        executable='spawn_entity.py',
        arguments=['-topic','robot_description','-entity','robot1'],
    )

    action_joint_state_controller=launch.actions.ExecuteProcess(
        cmd='ros2 control load_controller robot1_joint_state_broadcaster --set-state active'.split(' '),
        output='screen',
    )
    # action_obot1_joint_effort_controller=launch.actions.ExecuteProcess(
    #     cmd='ros2 control load_controller robot1_joint_effort_controller --set-state active'.split(' '),
    #     output='screen',
    # )
    action_robot1_diff_drive_controller=launch.actions.ExecuteProcess(
        cmd='ros2 control load_controller robot1_diff_drive_controller --set-state active'.split(' '),
        output='screen',
    )    
       
    

    #使用gazebo发布关节状态，就不需要joint_state_publisher模拟参数了
    # joint_state_publisher=launch_ros.actions.Node(
    #     package="joint_state_publisher",
    #     executable="joint_state_publisher",
    # )

    return launch.LaunchDescription([
        action_declare_arg_model_path,
        robot_state_publisher,
        action_gazebo_launch,
        launch_spawn_entity,
        launch.actions.RegisterEventHandler(
            event_handler=launch.event_handlers.OnProcessExit(
                target_action=launch_spawn_entity,
                on_exit=[action_joint_state_controller],
            )
        ),
                launch.actions.RegisterEventHandler(
            event_handler=launch.event_handlers.OnProcessExit(
                target_action=action_joint_state_controller,
                on_exit=[action_robot1_diff_drive_controller],
            )
        ),

    ]

    )